#include "../armor_detector/armor_detector.h"
#include <opencv2/opencv.hpp>
#include <queue>
#include <mutex>
#include <condition_variable>
#include <atomic>
#include <chrono>
#include <iostream>

#include "serial.h"

// 声明外部变量
extern std::atomic<bool> running;
extern std::atomic<bool> camReady;
extern std::queue<cv::Mat> frameQueue;
extern std::mutex queueMutex;
extern std::condition_variable queueCond;
extern std::queue<ReceivedData> receivedDataQueue;
extern std::mutex dataQueueMutex;
extern std::condition_variable dataQueueCond;

void armorDetectorThreadFunc() {
    // 等待相机和串口准备就绪
    while (running && !camReady) {
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
    }

    if (!running) return;

    // 初始化装甲板检测器
    ArmorDetector detector;
    detector.loadConfig("config/armor_detector.json");

    // FPS计算相关变量
    auto lastTime = std::chrono::steady_clock::now();
    int frameCount = 0;
    double fps = 0.0;

    // 处理时间计算
    auto processStartTime = std::chrono::steady_clock::now();
    double processTime = 0.0;

    // 最新接收到的串口数据
    ReceivedData latestData = {0.0, 0.0, 0.0, std::chrono::steady_clock::now()};

    while (running) {
        // 检查是否有新的图像帧
        cv::Mat frame;
        {
            std::lock_guard<std::mutex> lock(queueMutex);
            if (!frameQueue.empty()) {
                frame = frameQueue.front();
                frameQueue.pop();
            }
        }

        // 检查是否有新的串口数据
        {
            std::lock_guard<std::mutex> lock(dataQueueMutex);
            if (!receivedDataQueue.empty()) {
                latestData = receivedDataQueue.back(); // 获取最新的数据
                while (!receivedDataQueue.empty()) receivedDataQueue.pop(); // 清空队列
            }
        }

        if (!frame.empty()) {
            processStartTime = std::chrono::steady_clock::now();

            // 执行图像预处理（二值化）
            cv::Mat binaryImage = detector.preprocessImage(frame);

            // 查找灯条
            std::vector<LightBar> lights = detector.findLights(binaryImage, frame);

            // 匹配灯条形成装甲板（包含角点优化）
            std::vector<Armor> armors = detector.matchLights(frame, lights);

            // 识别装甲板上的数字
            detector.recognizeArmorNumbers(frame, armors);

            // 计算处理时间
            auto processEndTime = std::chrono::steady_clock::now();
            processTime = std::chrono::duration<double, std::milli>(processEndTime - processStartTime).count();

            // 计算FPS
            frameCount++;
            auto currentTime = std::chrono::steady_clock::now();
            double elapsedTime = std::chrono::duration<double>(currentTime - lastTime).count();
            if (elapsedTime >= 1.0) {
                fps = frameCount / elapsedTime;
                frameCount = 0;
                lastTime = currentTime;
            }

            // 计算数据延迟（当前时间与串口数据时间戳的差值）
            auto now = std::chrono::steady_clock::now();
            double dataDelay = std::chrono::duration<double, std::milli>(now - latestData.timestamp).count();

            // 创建一个副本来绘制装甲板（始终显示）
            cv::Mat drawingFrame = frame.clone();

            // 在原始图像上绘制检测到的装甲板和数字（始终显示）
            detector.drawArmors(drawingFrame, armors);

            // 在原始图像上添加信息
            cv::putText(drawingFrame,
                       "FPS: " + std::to_string(static_cast<int>(fps)),
                       cv::Point(800, 35),
                       cv::FONT_HERSHEY_SIMPLEX,
                       1.5,
                       cv::Scalar(0, 255, 0),
                       2);

            cv::putText(drawingFrame,
                       "Process Time: " + std::to_string(processTime) + " ms",
                       cv::Point(10, 35),
                       cv::FONT_HERSHEY_SIMPLEX,
                       1.5,
                       cv::Scalar(0, 255, 0),
                       2);
            //串口数据显示
            std::string serialInfo = "Serial - Y:" + std::to_string(latestData.yaw).substr(0, 6) +
                                   " P:" + std::to_string(latestData.pitch).substr(0, 4) +
                                   " R:" + std::to_string(latestData.roll).substr(0, 4) +
                                   " D:" + std::to_string(dataDelay).substr(0, 4) + "ms";
            cv::putText(drawingFrame,
                       serialInfo,
                       cv::Point(10, 75),
                       cv::FONT_HERSHEY_SIMPLEX,
                       1.5,
                       cv::Scalar(0, 255, 255),
                       2);

            // 显示灯条和装甲板数量
            cv::putText(drawingFrame,
                       "Lights: " + std::to_string(lights.size()) + " Armors: " + std::to_string(armors.size()),
                       cv::Point(10, 115),
                       cv::FONT_HERSHEY_SIMPLEX,
                       1.5,
                       cv::Scalar(0, 255, 255),
                       2);

            // 显示调试信息（根据配置文件决定是否显示）
            detector.showDebugInfo(frame, binaryImage, lights, armors);

            // 显示主结果图像
            cv::Mat resizedDrawingFrame;
            cv::resize(drawingFrame, resizedDrawingFrame, cv::Size(640, 480), 0, 0, cv::INTER_LINEAR);
            cv::imshow("Armor Detection Result", resizedDrawingFrame);

            // 检查按键输入，如果按下ESC键则退出程序
            int key = cv::waitKey(1) & 0xFF;
            if (key == 27) { // ESC键的ASCII码是27
                std::cout << "ESC key pressed. Exiting..." << std::endl;
                running = false;
                break;
            }
        } else {
            // 检查按键输入，如果按下ESC键则退出程序
            int key = cv::waitKey(1) & 0xFF;
            if (key == 27) { // ESC键的ASCII码是27
                std::cout << "ESC key pressed. Exiting..." << std::endl;
                running = false;
                break;
            }
        }
    }

    // 关闭所有OpenCV窗口
    cv::destroyAllWindows();
}
